#include "robot.h"
#include "UDP.h"

robot my_Robot = robot();
my_UDP my_udp = my_UDP();

void setup()
{
    Serial.begin(115200);//初始化波特率
    my_udp.my_UDP_init();//初始化udp
}
void loop()
{
	if(my_udp.UDP_rev())//如果成功解析则进行逆运动学解算并输出至电机
	{
	  Serial.printf("Receive from UDP v_x:%.2f v_z:%.2f\n", my_udp.rev_packet.v_x,my_udp.rev_packet.v_z);
	  my_Robot.ikine(my_udp.rev_packet.v_x,my_udp.rev_packet.v_z);
	}
}
